﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NatNetML;
using NatNetClient;
using System.Net;
using System.Windows;
using System.Threading;
using System.ComponentModel;
using System.IO.Ports;

namespace NatNetClient
{
    public class MyClientNatNet
    {
        public enum SENDMODE
        {
            STRING,
            BINARY,
            MAVLINK
        }
        private NatNetClientML _natNetClientML;
        private int _IdObj;
        private int _frequencyDivisor;

        private int _counterFrequency;
        private SENDMODE _mode;

        private ushort packetCount;


        //lista di socket a cui inviare i dati: prendo gli elementi dal dizionario
        List<UDPSender> _senders = new List<UDPSender>();
        //porta Seriale su cui mandare i pacchetti NatNet. Vale null se non è prevista come modalità.
        SerialPort _serial = null;
        SerialPort _serial2 = null;

        #region FieldUtility

        public event EventHandler OnUpdate;

        private int _packetSent;

        public int PacketSent
        {
            get { return _packetSent; }
            set
            {
                if (_packetSent == Int32.MaxValue)
                    _packetSent = 0;
                else
                    _packetSent = value;
            }
        }

        private int _packetReceived;

        public int PacketReceived
        {
            get { return _packetReceived; }
            set
            {
                if (_packetReceived == Int32.MaxValue)
                    _packetReceived = 0;
                else
                    _packetReceived = value;
            }
        }

        #endregion

        // iConnectionType = 0 indica trasmissione Multicast mentre uguale ad 1 indica su UDP
        public MyClientNatNet(int iConnectionType, SENDMODE mode)
        {
            _mode = mode;
            if (_natNetClientML != null)
            {
                _natNetClientML.Uninitialize();
            }
            _natNetClientML = new NatNetClientML(iConnectionType);

            _packetSent = 0;
            _packetReceived = 0;
        }

        public void Connect(string IpServer, int divFreq, int IdObjTrack, Dictionary<string, int> ipList, SerialPort serial, SerialPort serial2)
        {
            //CONTROLLO ARGOMENTI E INIZIALIZZO FIELD DELLA CLASSE COMUNI AD ENTRAMBE LE COMUNICAZIONI (UDP E SERIALE)
            InitConnect(IpServer, divFreq, IdObjTrack);

            if (ipList != null) //COMUNICAZIONE UDP ABILITATA
            {
                InitUdp(ipList);
            }
            if (serial != null) //COMUNICAZIONE SERIALE ABILITATA
            {
                InitSerialPort(serial);
            }
            else
            {
                _serial = null;
            }
            if (serial2 != null) //COMUNICAZIONE SERIALE ABILITATA
            {
                InitSerialPort2(serial2);
            }
            else
            {
                _serial2 = null;
            }

            //Funzione di callback invocata quando NatNet riceve dati provenienti
            //da una applicazione Server. Ricezione già a 100 Hz 
            _natNetClientML.OnFrameReady += new NatNetML.FrameReadyEventHandler(_natNetClientML_OnFrameReady);

        }

        private void InitSerialPort (SerialPort serial)
        {
            _serial = serial;
            _serial.Open();
        }
        private void InitSerialPort2(SerialPort serial)
        {
            _serial2 = serial;
            _serial2.Open();
        }

        private void InitUdp(Dictionary<string, int> ipList)
        {
            foreach (string ip in ipList.Keys)
            {
                UDPSender udp = new UDPSender(ip, ipList[ip]);
                _senders.Add(udp);
            }
        }

        private void InitConnect(string IpServer, int divFreq, int IdObjTrack)
        {
            #region ControlArgument

            if (IpServer == String.Empty || IpServer == null)
                throw new ArgumentNullException("IpServer");
            if (divFreq < 0)
                throw new ArgumentNullException("divFreq");
            if (IdObjTrack < 0)
                throw new ArgumentNullException("IdObjTrack");

            //Recupero il mio IP
            IPAddress myAddress = null;

            IPAddress[] localAddress = Dns.GetHostAddresses(Dns.GetHostName());
            //i primi 4 in questo caso sono IPv6!! in questo caso...?
            foreach (IPAddress addr in localAddress)
            {
                if (addr.AddressFamily == System.Net.Sockets.AddressFamily.InterNetwork)
                    myAddress = addr;
            }

            //IpServer

            IPAddress ipTrackingTools;
            if (!IPAddress.TryParse(IpServer, out ipTrackingTools))
            {
                throw new ArgumentException("ipTrackingTools is not well formed");
            }

            #endregion
            //questo metodo inizializza le socket del Client e cerca di connettersi al Server specificato
            //_natNetClientML.Initialize(myAddress.ToString(), IpServer);
            _natNetClientML.Initialize(IpServer, IpServer);
            _frequencyDivisor = divFreq;
            _IdObj = IdObjTrack;
            _counterFrequency = 0;
        }

        void _natNetClientML_OnFrameReady(FrameOfMocapData data, NatNetClientML client)
        {
            PacketReceived++;
            _counterFrequency++;
            foreach (RigidBodyData body in data.RigidBodies)
            {
                if (body.ID == _IdObj)  // è l' oggetto d' interesse e raccolgo le informazioni
                {
                    short x = (short)Math.Truncate(body.x * 1000);
                    short y = (short)Math.Truncate(body.y * 1000);
                    short z = (short)Math.Truncate(body.z * 1000);

                    short qx = (short)Math.Truncate(body.qx * 10000);
                    short qy = (short)Math.Truncate(body.qy * 10000);
                    short qz = (short)Math.Truncate(body.qz * 10000);
                    short qw = (short)Math.Truncate(body.qw * 10000);

                    short meanError = (short)Math.Truncate(body.MeanError * 1000);


                    //calcolo del timeStamp
                    long millisecondi = DateTime.Now.TimeOfDay.Ticks / 1000;

                    long t = (Convert.ToInt64(Math.Truncate(data.fLatency * 10000)) % 60000);
                    ushort timeStamp = Convert.ToUInt16(t);


                    OptiTrackPacket packet = new OptiTrackPacket(x, y, z, qx, qy, qz, qw, meanError, timeStamp, packetCount);

                    packetCount++;

                    if (packetCount > 10000)
                    {
                        packetCount = 0;
                    }
                    //in caso di divisore di frequenza: scarto i pacchetti
                    // Esempio: divisore per 2 = 50 Hz, un pacchetto lo scarto e uno lo invio
                    if (_counterFrequency == _frequencyDivisor)
                    {
                        _counterFrequency = 0;

                        foreach (UDPSender udp in _senders)
                        {
                            udp.Send(packet,_mode);
                        }
                        if (_serial != null)
                        {
                            byte[] sstream = new byte[0];
                            switch (_mode)
                            {
                                case SENDMODE.STRING:
                                    sstream = Encoding.Default.GetBytes(packet.ToString());
                                    break;
                                case SENDMODE.BINARY:
                                    sstream = packet.ToBinary();
                                    break;
                                case SENDMODE.MAVLINK:
                                    sstream = packet.ToMavlink();
                                    break;

                            }
                            _serial.Write(sstream, 0, sstream.Length);
                        }
                        if (_serial2 != null)
                        {
                            byte[] sstream;
                            Encoding enc = Encoding.UTF8;
                            sstream = enc.GetBytes(packet.ToString());
                            _serial2.Write(sstream, 0, sstream.Length);
                        }
                        PacketSent++;
                    }
                    OnUpdate(this, EventArgs.Empty);
                    return;
                }
            }
        }

        //invio richiesta al Server per la ricezione di informazioni su di esso
        public string ServerDescription()
        {
            ServerDescription desc = new ServerDescription();
            int returnCode = _natNetClientML.GetServerDescription(desc);
            StringBuilder strb = new StringBuilder();

            if (returnCode == 0)
            {
                strb = strb.Append("Connection Succeeded.");
                strb = strb.Append("   Server App Name: " + desc.HostApp);
                strb = strb.Append(String.Format("   Server App Version: {0}.{1}.{2}.{3}", desc.HostAppVersion[0], desc.HostAppVersion[1], desc.HostAppVersion[2], desc.HostAppVersion[3]));
                strb = strb.Append(String.Format("   Server NatNet Version: {0}.{1}.{2}.{3}", desc.NatNetVersion[0], desc.NatNetVersion[1], desc.NatNetVersion[2], desc.NatNetVersion[3]));

                return strb.ToString();
            }
            else
            {
                return "Error Connecting.";
            }
        }

        public void Disconnect()
        {
            if (_natNetClientML != null)
            {
                _natNetClientML.Uninitialize();
            }
            //chiudo ogni socket
            foreach (UDPSender sender in _senders)
            {
                sender.Close();
            }
            if (_serial != null)
            {
                _serial.Close();
            }
            if (_serial2 != null)
            {
                _serial2.Close();
            }

            OnUpdate(this, EventArgs.Empty);

        }
    }
}
